#include "PsoSharingMethods.h"
#include "MathUtil.h"

#include <math.h>

namespace jcrada {

    RingSharingMethod::RingSharingMethod( int neighbors ) : PsoSharingMethod( ),
    _neighbors( neighbors ) {

    }

    void RingSharingMethod::share( PsoModel& model ) {
        int n = getNeighbors() / 2;
        PsoParticle * particle = NULL;
        PsoParticle * lbest = NULL;
        double lbest_fitness;
        for (int i = 0; i < model.getSize(); ++i) {
            lbest = NULL;
            lbest_fitness = model.isLowerFitnessBetter() ? 1.0 / 0.0 : -1.0 / 0.0;
            for (int j = -n; j < n; ++j) {
                particle = &model.getParticle(Math::modEuclidean(i + j, model.getSize()));
                if (model.isBetter(particle->getFitness(), lbest_fitness)) {
                    lbest = particle;
                    lbest_fitness = lbest->getFitness();
                }
            }
            assert(lbest);
            for (int j = -n; j < n; ++j) {
                particle = &model.getParticle(Math::modEuclidean(i + j, model.getSize()));
                for (int k = 0; k < particle->getSize(); ++k) {
                    particle->setBestNeighborhoodPosition(k, lbest->getPosition(k));
                }
                particle->setFitnessForNeighborhoodBest(lbest->getFitness());
            }
        }
    }

    //STAR SharingMethod

    void StarSharingMethod::share( PsoModel& model ) {
        double gbest_fitness = model.isLowerFitnessBetter() ? 1.0 / 0.0 : -1.0 / 0.0;
        PsoParticle * gbest = NULL;
        for (int i = 0; i < model.getSize(); ++i) {
            if (model.isBetter(model.getParticle(i).getFitness(), gbest_fitness)) {
                gbest = &model.getParticle(i);
                gbest_fitness = gbest->getFitness();
            }
        }
        assert(gbest);
        for (int i = 0; i < model.getSize(); ++i) {
            for (int j = 0; j < model.getParticle(i).getSize(); ++j) {
                model.getParticle(i).setBestNeighborhoodPosition(j, gbest->getPosition(j));
            }
            model.getParticle(i).setFitnessForNeighborhoodBest(gbest->getFitness());
        }
    }

    //VON NEUMANN SharingMethod

    bool VonNeumannSharingMethod::isValidIndex( int size, int index ) {
        return index < size && index >= 0;
    }

    void VonNeumannSharingMethod::findBestVonNeumannNeighbors( PsoModel& model, int cubes, PsoParticle**** vneumann, PsoParticle** allnbest ) {
        int index = 0;
        double fitness;
        PsoParticle * nbest = NULL;
        for (int i = 0; i < cubes; ++i) {
            for (int j = 0; j < cubes; ++j) {
                fitness = model.isLowerFitnessBetter() ? 1.0 / 0.0 : -1.0 / 0.0;
                nbest = NULL;
                for (int k = 0; k < cubes; ++k) {
                    if (isValidIndex(cubes, i - 1)) {
                        if (model.isBetter(vneumann[i - 1][j][k]->getFitness(), fitness)) {
                            fitness = vneumann[i - 1][j][k]->getFitness();
                            nbest = vneumann[i - 1][j][k];
                        }
                    }
                    if (isValidIndex(cubes, i + 1)) {
                        if (model.isBetter(vneumann[i + 1][j][k]->getFitness(), fitness)) {
                            fitness = vneumann[i + 1][j][k]->getFitness();
                            nbest = vneumann[i + 1][j][k];
                        }
                    }
                    if (isValidIndex(cubes, j - 1)) {
                        if (model.isBetter(vneumann[i][j - 1][k]->getFitness(), fitness)) {
                            fitness = vneumann[i][j - 1][k]->getFitness();
                            nbest = vneumann[i][j - 1][k];
                        }
                    }
                    if (isValidIndex(cubes, j + 1)) {
                        if (model.isBetter(vneumann[i][j + 1][k]->getFitness(), fitness)) {
                            fitness = vneumann[i][j + 1][k]->getFitness();
                            nbest = vneumann[i][j + 1][k];
                        }
                    }
                    if (isValidIndex(cubes, k - 1)) {
                        if (model.isBetter(vneumann[i][j][k - 1]->getFitness(), fitness)) {
                            fitness = vneumann[i][j][k - 1]->getFitness();
                            nbest = vneumann[i][j][k - 1];
                        }
                    }
                    if (isValidIndex(cubes, k + 1)) {
                        if (model.isBetter(vneumann[i][j][k + 1]->getFitness(), fitness)) {
                            fitness = vneumann[i][j][k + 1]->getFitness();
                            nbest = vneumann[i][j][k + 1];
                        }
                    }
                    allnbest[index++] = nbest;
                }
            }
        }
    }

    void VonNeumannSharingMethod::share( PsoModel& model ) {
        int cubes = cbrt(model.getSize());
        PsoParticle **** vneumann = new PsoParticle***[cubes];
        int index = 0;
        for (int i = 0; i < cubes; ++i) {
            vneumann[i] = new PsoParticle**[cubes];
            for (int j = 0; j < cubes; ++j) {
                vneumann[i][j] = new PsoParticle*[cubes];
                for (int k = 0; k < cubes; ++k) {
                    vneumann[i][j][k] = &model.getParticle(index++);
                }
            }
        }

        PsoParticle ** allnbest = new PsoParticle *[model.getSize()];
        findBestVonNeumannNeighbors(model, cubes, vneumann, allnbest);
        index = 0;
        for (int i = 0; i < cubes; ++i) {
            for (int j = 0; j < cubes; ++j) {
                for (int k = 0; k < cubes; ++k) {
                    if (isValidIndex(cubes, i - 1)) {
                        vneumann[i - 1][j][k]->setBestNeighborhoodPosition(allnbest[index]->getPosition());
                        vneumann[i - 1][j][k]->setFitnessForNeighborhoodBest(allnbest[index]->getFitness());
                    }
                    if (isValidIndex(cubes, i + 1)) {
                        vneumann[i + 1][j][k]->setBestNeighborhoodPosition(allnbest[index]->getPosition());
                        vneumann[i + 1][j][k]->setFitnessForNeighborhoodBest(allnbest[index]->getFitness());
                    }
                    if (isValidIndex(cubes, j - 1)) {
                        vneumann[i][j - 1][k]->setBestNeighborhoodPosition(allnbest[index]->getPosition());
                        vneumann[i][j - 1][k]->setFitnessForNeighborhoodBest(allnbest[index]->getFitness());
                    }
                    if (isValidIndex(cubes, j + 1)) {
                        vneumann[i][j + 1][k]->setBestNeighborhoodPosition(allnbest[index]->getPosition());
                        vneumann[i][j + 1][k]->setFitnessForNeighborhoodBest(allnbest[index]->getFitness());
                    }
                    if (isValidIndex(cubes, k - 1)) {
                        vneumann[i][j][k - 1]->setBestNeighborhoodPosition(allnbest[index]->getPosition());
                        vneumann[i][j][k - 1]->setFitnessForNeighborhoodBest(allnbest[index]->getFitness());
                    }
                    if (isValidIndex(cubes, k + 1)) {
                        vneumann[i][j][k + 1]->setBestNeighborhoodPosition(allnbest[index]->getPosition());
                        vneumann[i][j][k + 1]->setFitnessForNeighborhoodBest(allnbest[index]->getFitness());
                    }
                }
                index++;
            }
        }
    }
}
